//
// Created by wangyihu2020 on 2022/6/26.
//

#include <Types.h>
#include <string.h>
#include <iostream>
#include <mallocMatrix.h>
#include <math.h>
#include <Calculator.h>
Coord::Coord() {
    m_v[0] = 0;
    m_v[1] = 0;
    m_v[2] = 0;
}

Coord::Coord(qp_float v1, qp_float v2, qp_float v3) {
    m_v[0] = v1;
    m_v[1] = v2;
    m_v[2] = v3;
}

Coord::Coord(Coord const & v) {
    m_v[0] = v.m_v[0];
    m_v[1] = v.m_v[1];
    m_v[2] = v.m_v[2];
}

Coord Coord::operator+(Coord const& v) {
    return {m_v[0] + v.m_v[0], m_v[1] + v.m_v[1], m_v[2] + v.m_v[2]};
}

Coord Coord::operator-(Coord const& v) {
    return {m_v[0] - v.m_v[0], m_v[1] - v.m_v[1], m_v[2] - v.m_v[2]};
}

qp_float Coord::operator*(Coord const& v) {
    return (m_v[0] * v.m_v[0])+ (m_v[1] * v.m_v[1]) + (m_v[2] * v.m_v[2]);
}

qp_float Coord::operator[](qp_int i) {
    return m_v[i];
}

Coord& Coord::operator=(Coord const& v) {
    if(this == &v) return *this;
    m_v[0] = v.m_v[0];
    m_v[1] = v.m_v[1];
    m_v[2] = v.m_v[2];
    return *this;
}

Coord Coord::operator+(const qp_float * v) {
    return {m_v[0] + v[0], m_v[1] + v[1], m_v[2] + v[2]};
}

Coord Coord::operator-(const qp_float * v) {
    return {m_v[0] - v[0], m_v[1] + v[1], m_v[2] + v[2]};
}

qp_float Coord::operator*(const qp_float * v) {
    return m_v[0] * v[0] + m_v[1] * v[1] + m_v[2] * v[2];
}

Coord& Coord::operator=(const qp_float * v) {
    m_v[0] = v[0];
    m_v[1] = v[1];
    m_v[2] = v[2];
    return *this;
}


//***************************************************************************************************************

Mat::Mat(){
    m_row = 0;
    m_col = 0;
    m_dep = 0;
    m_dim = 0;
}

Mat::Mat(qp_int v1, qp_int v2, qp_int v3) {
    m_row = v1;
    m_col = v2;
    m_dep = v3;
    if(v1 == 0) {
        m_dim = 0;
        return;
    }
    if(v2 == 0){
        m_dim = 1;
        m_elements = std::vector<qp_float>(m_row, 0);
        return;
    }
    if(v3 == 0){
        m_dim = 2;

        m_v.resize(v1);
        for(int i = 0; i < m_row; i++){
            m_v[i].m_row = m_col;
            m_v[i].m_col = 0;
            m_v[i].m_dep = 0;
            if(i == 500)
                int aa = 4;
            if(i == 5000)
                int aa = 4;
            if(i == 10000)
                int aa = 4;
            if(i == 30000)
                int aa = 4;
            if(i == 50000)
                int aa = 4;
            if(i == 80000)
                int aa = 4;
            m_v[i].m_dim = 1;
            m_v[i].m_elements.resize(v2);
        }
//        m_v = std::vector<Mat>(m_row, Mat(m_col));
//        m_v.shrink_to_fit();
        return;
    }
    m_dim = 3;
    m_v = std::vector<Mat>(m_row, Mat(m_col, m_dep));
    m_v.shrink_to_fit();
}

//Mat::Mat(qp_int row, qp_int col) {
//    m_row = row;
//    m_col = col;
//    m_v = callocMatrix<qp_float>(row, col);
//}

void Mat::init(qp_int row, qp_int col, qp_int dep) {
    m_row = row;
    m_col = col;
    m_dep = dep;
    if(row == 0) {
        m_dim = 0;
        m_v.clear();
        m_v.shrink_to_fit();
        return;
    }
    if(col == 0){
        m_dim = 1;
        m_v.clear();
        m_v.shrink_to_fit();
        m_elements = std::vector<qp_float>(m_row, 0);
        return;
    }
    if(dep == 0){
        m_dim = 2;
        m_v = std::vector<Mat>(m_row, Mat(m_col));
        m_v.shrink_to_fit();
        return;
    }
    m_dim = 3;
    m_v = std::vector<Mat>(m_row, Mat(m_col, m_dep));
    m_v.shrink_to_fit();
}
Mat::~Mat(){
//    freeMatrix(m_v, m_row, m_col);
}

Mat::Mat(Mat const & v) {
    m_row = v.m_row;
    m_col = v.m_col;
    m_dep = v.m_dep;
    m_dim = v.m_dim;
    m_v = v.m_v;
    m_elements = v.m_elements;
}

Mat::Mat(Mat && v) {
    m_row = v.m_row;
    m_col = v.m_col;
    m_dep = v.m_dep;
    m_dim = v.m_dim;
    m_v = v.m_v;
    m_elements = v.m_elements;
}


Mat& Mat::operator+=(Mat const& v) {
    for(int i = 0; i < getRow(); i++){
        if(m_dim != 1)
            m_v[i] += v.m_v[i];
        else m_elements[i] += v.m_elements[i];
    }
    return *this;
}

Mat& Mat::operator+=(const qp_float *v) {
    for(int i = 0; i < getRow(); i++){
        if(m_dim == 2)
            for(int j = 0; j < getCol(); j++){
                if(m_dim == 3)
                    for(int k = 0; k < getDep(); k++){
                        m_v[i](j)[k] += v[i * m_row + j * m_col + k];
                    }
                else m_v[i][j] += v[i * m_row + j * m_col];
            }
        else m_elements[i] += v[i];
    }
    return *this;
}

Mat&& Mat::operator+(Mat const& v) {
    Mat* newMat = new Mat(*this);
    for(int i = 0; i < getRow(); i++){
        if(m_dim != 1)
            m_v[i] += v.m_v[i];
        else m_elements[i] += v.m_elements[i];
    }
    return std::move(*newMat);
}

Mat&& Mat::operator+(const qp_float *v) {
    Mat* newMat = new Mat(*this);
    for(int i = 0; i < getRow(); i++){
        if(m_dim == 2)
            for(int j = 0; j < getCol(); j++){
                if(m_dim == 3)
                    for(int k = 0; k < getDep(); k++){
                        newMat->m_v[i](j)[k] += v[i * m_row + j * m_col + k];
                    }
                else newMat->m_v[i][j] += v[i * m_row + j * m_col];
            }
        else newMat->m_elements[i] += v[i];
    }
    return std::move(*newMat);
}

Mat& Mat::operator-=(Mat const& v) {
    for(int i = 0; i < getRow(); i++){
        if(m_dim != 1)
            m_v[i] -= v.m_v[i];
        else m_elements[i] -= v.m_elements[i];
    }
    return *this;
}

Mat& Mat::operator-=(const qp_float *v) {
    for(int i = 0; i < getRow(); i++){
        if(m_dim == 2)
            for(int j = 0; j < getCol(); j++){
                if(m_dim == 3)
                    for(int k = 0; k < getDep(); k++){
                        m_v[i](j)[k] -= v[i * m_row + j * m_col + k];
                    }
                else m_v[i][j] -= v[i * m_row + j * m_col];
            }
        else m_elements[i] -= v[i];
    }
    return *this;
}

Mat&& Mat::operator-(Mat const& v) {
    Mat* newMat = new Mat(*this);
    for(int i = 0; i < getRow(); i++){
        if(m_dim != 1)
            newMat->m_v[i] -= v.m_v[i];
        else newMat->m_elements[i] -= v.m_elements[i];
    }
    return std::move(*newMat);
}

Mat&& Mat::operator-(const qp_float *v) {
    Mat* newMat = new Mat(*this);
    for(int i = 0; i < getRow(); i++){
        if(m_dim == 2)
            for(int j = 0; j < getCol(); j++){
                if(m_dim == 3)
                    for(int k = 0; k < getDep(); k++){
                        newMat->m_v[i](j)[k] -= v[i * m_row + j * m_col + k];
                    }
                else newMat->m_v[i][j] -= v[i * m_row + j * m_col];
            }
        else newMat->m_elements[i] -= v[i];
    }
    return std::move(*newMat);
}
Mat&& Mat::operator*(Mat & v) {
    if(m_dim == 3){
        //todo
        return std::move(Mat());
    }
    bool signal = true;
    if (getCol() != v.getRow()) {
        if(getCol() != v.getCol()){
            std::cout << "error in Mat multiply" << std::endl;
            return std::move(Mat());
        }
        else signal = false;
    }

    Mat* newmat;
    if(signal == true) {
        newmat = new Mat(m_row, v.getCol());
        for (int i = 0; i < m_row; i++) {
            if(v.m_dim != 1)
                for (int j = 0; j < v.getCol(); j++) {
                    for (int k = 0; k < m_col; k++) {
                        (*newmat)(i)[j] += m_v[i][k] * v(k)[j];
                    }
                }
            else{
                for(int k = 0; k < m_col; k++){
                    (*newmat)[i] += m_v[i][k] * v[k];
                }
            }
        }
    }
    else{
        newmat = new Mat(m_row, v.getRow());
        for (int i = 0; i < m_row; i++) {
            for (int j = 0; j < v.getRow(); j++) {
                for (int k = 0; k < m_col; k++) {
                    (*newmat)(i)[j] += m_v[i][k] * v(j)[k];
                }
            }
        }
    }
    return std::move(*newmat);
}

Mat&& Mat::operator*(Mat && v) {
    bool signal = true;
    if (getCol() != v.getRow()) {
        if(getCol() != v.getCol()){
            std::cout << "error in Mat multiply" << std::endl;
            return std::move(Mat());
        }
        else signal = false;
    }
    Mat* newmat;
    if(signal == true) {
        newmat = new Mat(m_row, v.getCol());
        for (int i = 0; i < m_row; i++) {
            if(v.m_dim != 1)
                for (int j = 0; j < v.getCol(); j++) {
                    for (int k = 0; k < m_col; k++) {
                        (*newmat)(i)[j] += m_v[i][k] * v(k)[j];
                    }
                }
            else{
                for(int k = 0; k < m_col; k++){
                    (*newmat)[i] += m_v[i][k] * v[k];
                }
            }
        }
    }
    else{
        newmat = new Mat(m_row, v.getRow());
        for (int i = 0; i < m_row; i++) {
            for (int j = 0; j < v.getRow(); j++) {
                for (int k = 0; k < m_col; k++) {
                    (*newmat)(i)[j] += m_v[i][k] * v(j)[k];
                }
            }
        }
    }
    return std::move(*newmat);
}

Mat&& Mat::operator*(qp_float v) {
    Mat* newmat;
    newmat = new Mat(m_row, m_col, m_dep);
    for (int i = 0; i < m_row; i++) {
        if(m_col != 0)
            for (int j = 0; j < m_col; j++) {
                (*newmat)(i)[j] *= v;
            }
        else
            (*newmat)[i] *= v;

    }
    return std::move(*newmat);
}

Mat& Mat::operator*=(qp_float v) {
    for (int i = 0; i < m_row; i++) {
        if(m_col != 0)
            for (int j = 0; j < m_col; j++) {
                m_v[i][j] *= v;
            }
        else
            m_elements[i] *= v;

    }
    return *this;
}
//
qp_float& Mat::operator[](qp_int i) {
    return m_elements[i];
}

qp_float Mat::operator[](qp_int i) const{
    return m_elements[i];
}

//
Mat& Mat::operator=(Mat const& v) {
    if(this == &v) return *this;
    m_row = v.m_row;
    m_col = v.m_col;
    m_dep = v.m_dep;
    m_dim = v.m_dim;
    m_v = v.m_v;
    m_elements = v.m_elements;
    return *this;
}

Mat& Mat::operator=(Mat&& v)  noexcept {
    m_row = v.m_row;
    m_col = v.m_col;
    m_dep = v.m_dep;
    m_dim = v.m_dim;
    m_v = v.m_v;
    m_elements = v.m_elements;
    return *this;
}

Mat& Mat::operator=(const qp_float * v) {
    if(m_dim == 3){
        //todo
        return *this;
    }
    if(m_dim == 2)
    for(int i = 0; i < getRow(); i++){
        for(int j = 0; j < getCol(); j++){
            m_v[i][j] = v[i * m_col + j];
        }
    }
    if(m_dim == 1){
        for(int i = 0; i < getRow(); i++){
            m_elements[i] = v[i];
        }
    }
    return *this;
}

Mat &Mat::operator=(std::initializer_list<qp_float> v) {
    if(m_dim == 3){
        //todo
        return *this;
    }
    if(m_row * m_col != v.size()) {
        std::cout<<"Mat init with list failed"<<std::endl;
        return *this;
    }
    auto it = v.begin();
    for(int i = 0; i < m_row; i++){
        for(int j = 0; j < m_col; j++){
            m_v[i][j] = *it++;
        }
    }
    return *this;
}

Mat& Mat::operator()(qp_int row, qp_int col){
    if(col == -1)
        return m_v[row];
    else return m_v[row](col);
}

qp_int Mat::getRow() const{
    return m_row;
}

qp_int Mat::getCol() const{
    return m_col;
}

qp_int Mat::getDep() const{
    return m_dep;
}

void Mat::setRow(qp_int m) {
    m_row = m;
}

void Mat::setCol(qp_int m) {
    m_col = m;
}



void Mat::print() {
    for(int i = 0; i < m_row; i++){
        for(int j = 0; j < m_col; j++){
            std::cout<<m_v[i][j]<<' ';
        }
        std::cout<<std::endl;
    }

}

void Mat::toArray(qp_float* v) {
    if(m_dim == 3){
        //todo
        return;
    }
    for(int i = 0; i < m_row; i++){
        for(int j = 0; j < m_col; j++)
            v[i* m_col + j] = m_v[i][j];
    }
//    std::cout<<&(v[0])<<' '<<&(v[1])<<' '<<&(v[2])<<' '<<std::endl;
//    return (qp_float**)newv;
}


void Mat::transpose(Mat & v) {
    if(m_dim == 3)
        return;
    if(this == &v){
        for (int i=0; i<m_row; i++)
        {
            for (int j=i+1; j<m_col; j++)
            {
                std::swap(this->m_v[i][j], this->m_v[j][i]);
            }
        }
    }
    else{
        if(!m_v.empty())
            init(v.getRow(), v.getCol());
        else clear();
        for (int i=0; i<m_row; i++)
            for (int j=0; j<m_col; j++)
                this->m_v[i][j]=v(j)[i];
    }
}

void Mat::clear() {
    m_row = 0;
    m_col = 0;
    m_dep = 0;
    m_dim = 0;
    m_v.clear();
    m_elements.clear();
    m_v.shrink_to_fit();
    m_elements.shrink_to_fit();
}

void Mat::assignMat(qp_float * v) {
    for(int i = 0; i < m_row; i++){
        if(m_dim == 2)
            for(int j = 0; j < m_col; j++){
                v[i * m_col + j] = m_v[i][j];
            }
        else v[i] = m_elements[i];
    }
}

void Mat::Subtracted(qp_float * v) {
    for(int i = 0; i < m_row; i++){
        if(m_dim == 2)
            for(int j = 0; j < m_col; j++){
                v[i * m_col + j] -= m_v[i][j];
            }
        else v[i] = m_elements[i];
    }
}

Mat& Mat::normalize() {
    if(m_dim != 1) return *this;
    qp_float norm = Calculator::calcNorm(*this, m_row);
    for(int i = 0; i < m_row; i++){
        m_elements[i] /= norm;
    }
    return *this;
}

void Mat::addtoArray(qp_float * v) {
    if(m_dim != 1) return;
    for(int i = 0; i < m_row; i++){
        v[i] += m_elements[i];
    }
}






//****************************************************************************


